from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
import os
import math

def generate_launch_description():

    return LaunchDescription([

        DeclareLaunchArgument(
            'ugv_ids',
            default_value="['ugv1']",
            description='List of UGV IDs to bridge, e.g. ["ugv1", "ugv2"]'
        ),

        Node(package='sls_launch_sim',
            executable='position_publisher',
            name='position_publisher',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),

        Node(
            package='sls_launch_sim',
            executable='relay_node',
            name='relay_node',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),


        Node(
            package='sls_launch_sim',
            executable='chassis_cmd_node',
            name='chassis_cmd_node',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),
        Node(
            package='sls_launch_sim',
            executable='motor_state_bridge',
            name='motor_state_bridge',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),
        Node(
            package='sls_launch_sim',
            executable='steering_bridge',
            name='steering_bridge',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),
        Node(
            package='sls_launch_sim',
            executable='obstacle_avoider',
            name='obstacle_avoider',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),


    ])
